//
// Created by bobin on 17-11-11.
//

#ifndef PNPL_PLUCKER_H
#define PNPL_PLUCKER_H

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include "opencv2/core/core.hpp"

namespace g2o {

    typedef Eigen::Matrix<double, 6, 1, Eigen::ColMajor> Vector6D;

    class VertexPluckerLine : public BaseVertex<6, Vector6D> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        VertexPluckerLine();

        virtual bool read(std::istream &is);

        virtual bool write(std::ostream &os) const;

        virtual void setToOriginImpl() {
            _estimate.fill(0.);
        }

        virtual void oplusImpl(const double *update) {
            Eigen::Map<const Vector6D> v(update);
            _estimate += v;
        }
    };

    typedef Eigen::Matrix<double, 3, 6> PluckerMatrix;

    class EdgePluckerLine : public BaseBinaryEdge<4, Vector4D, VertexPluckerLine, VertexSE3Expmap> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

        EdgePluckerLine();

        bool read(std::istream &is);

        bool write(std::ostream &os) const;

        void computeError() {
            const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]);
            const VertexPluckerLine *v2 = static_cast<const VertexPluckerLine *>(_vertices[0]);
            const CameraParameters *cam
                    = static_cast<const CameraParameters *>(parameter(0));
            Vector4D obs(_measurement);

            Vector3D start, end;
            start[2] = 1;
            end[2] = 1;
            start.head<2>() = ((obs.head<2>() - cam->principle_point) / cam->focal_length);
            end.head<2>() = ((obs.tail<2>() - cam->principle_point) / cam->focal_length);
            Vector6D est = v2->estimate();
            Vector3D line = start.cross(end);
            double a = line[0];
            double b = line[1];
            double c = line[2];

            PluckerMatrix matrix;
            SE3Quat se3Quat = v1->estimate();
            Matrix3D rot = se3Quat.rotation().normalized().toRotationMatrix();
            Vector3D trans = se3Quat.translation();
            matrix.block<3, 3>(0, 0) = rot;
            Matrix3D t_hat;
            t_hat << 0, -trans[2], trans[1],
                    trans[2], 0, -trans[0],
                    -trans[1], trans[0], 0;
            matrix.block<3, 3>(0, 3) = t_hat * rot;
            Vector3D line_est = matrix * est;

            double a_est = line_est[0];
            double b_est = line_est[1];
            double c_est = line_est[2];

            _error[0] = 0;//c * a_est - a * c_est;
            _error[1] = 0;//c * b_est - b * c_est;
            _error[2] = start.dot(line_est);
            _error[3] = end.dot(line_est);

        }

        CameraParameters *_cam;
    };

}


void PnPL_plucker(const std::vector<cv::Point3f> &pts3d, const std::vector<cv::Point2f> &pts2d,
                  const std::vector<cv::Vec6f> &lns3d, const std::vector<cv::Vec4f> &lns2d,
                  const cv::Mat &K,
                  cv::Mat &R, cv::Mat &t);


#endif //PNPL_PLUCKER_H
